#include <iostream>
#include <memory>
#include <string>
#include <chrono>
#include <thread>
#include <functional>
#include <behaviortree_cpp_v3/bt_factory.h>
#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/string.hpp>
using namespace BT;

class SaySomething : public SyncActionNode
{
public:
    SaySomething(const std::string& name, const NodeConfiguration& config)
        : SyncActionNode(name, config) {}

    static PortsList providedPorts()
    {
        return { InputPort<std::string>("message") };
    }

    NodeStatus tick() override
    {
        auto msg = getInput<std::string>("message");
        if (!msg)
        {
            throw BT::RuntimeError("missing required input [message]: ", msg.error());
        }
        std::cout  << msg.value() << std::endl;
        return NodeStatus::SUCCESS;
    }
};

class ROS2Node : public rclcpp::Node
{
public:
    ROS2Node() : Node("bt_ros2_node")
    {
        publisher_ = this->create_publisher<std_msgs::msg::String>("bt_status", 10);
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&ROS2Node::timer_callback, this));
    }

    void publish_status(const std::string &status)
    {
        auto message = std_msgs::msg::String();
        message.data = status;
        publisher_->publish(message);
    }

private:
    void timer_callback()
    {
        publish_status("Behavior Tree running...");
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    // Create the ROS2 Node
    auto ros_node = std::make_shared<ROS2Node>();

    // Create the Behavior Tree
    BehaviorTreeFactory factory;
    factory.registerNodeType<SaySomething>("SaySomething");

    auto tree = factory.createTreeFromFile("my_tree.xml");

    // Spin in a separate thread
    std::thread ros_thread([&]() { rclcpp::spin(ros_node); });

    while (rclcpp::ok())
    {
        tree.tickRoot();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    ros_thread.join();
    rclcpp::shutdown();

    return 0;
}
